#include "Object.h"

Object::Object()
{
	centroid = Point(0, 0);
	topLeft = Point(0, 0);
	downRight = Point(0, 0);
	width = 0;
	height = 0;
	vX = 0;
	vY = 0;
}

Object::Object( Point topLeft, Point downRight )
{
	if(topLeft.x > downRight.x || topLeft.y > downRight.y || topLeft.x < 0 || topLeft.y < 0 || downRight.x < 0 || downRight.y < 0)
	{
		cout<<"invalid initial top_left and down_right!"<<endl;
	}else
	{
		this->topLeft.x = topLeft.x;
		this->topLeft.y = topLeft.y;
		this->downRight.x = downRight.x;
		this->downRight.y = downRight.y;

		this->centroid.x = (topLeft.x + downRight.x) / 2;
		this->centroid.y = (topLeft.y + downRight.y) / 2;

		width = downRight.x - topLeft.x;
		height = downRight.y - topLeft.y;

		vX = 0;
		vY = 0;
	}	
}

Object::Object( unsigned int minx, unsigned int miny, unsigned int maxx, unsigned int maxy )
{
	if(minx > maxx || miny > maxy || minx < 0 || miny < 0 || maxx < 0 || maxy < 0)
	{
		cout<<"invalid initial blob's value!"<<endl;
	}else
	{
		this->topLeft.x = minx;
		this->topLeft.y = miny;
		this->downRight.x = maxx;
		this->downRight.y = maxy;

		this->centroid.x = (minx + maxx) / 2;
		this->centroid.y = (miny + maxy) / 2;

		width = maxx - minx;
		height = maxy - miny;

		vX = 0;
		vY = 0;
	}	
}

Object::~Object()
{

}

void Object::predict( Object* allObject, int numObject )
{
	centroidPredict.x = centroid.x + vX;
	centroidPredict.y = centroid.y + vY;

	float minDis = (allObject[0].centroid.x - centroidPredict.x)*(allObject[0].centroid.x - centroidPredict.x) + 
					(allObject[0].centroid.y - centroidPredict.y)*(allObject[0].centroid.y - centroidPredict.y);
	float value;
	int minIndex = 0;

	for(int i = 1; i < numObject; i++)
	{
		float value = (allObject[i].centroid.x - centroidPredict.x)*(allObject[i].centroid.x - centroidPredict.x) + 
			(allObject[i].centroid.y - centroidPredict.y)*(allObject[i].centroid.y - centroidPredict.y);
		if(value < minDis)
		{
			minDis = value;
			minIndex = i;
		}
	}

	centroidPredict.x = allObject[minIndex].centroid.x;
	centroidPredict.y = allObject[minIndex].centroid.y;
}

void Object::measure( Mat preFrame, Mat frame, Object* allObject, int numObject )
{
	const int RADIUS_MOVING = 100;

	int distance = 0;
	float correValue = 0.0;
	float maxCorre = 0.0;
	int indexMax = -1;
	Mat roiCandidateObject;
	Mat roiObject;

	roiObject = preFrame(Rect(topLeft.x, topLeft.y, width, height));
	imshow("template image", roiObject);
	for(int i = 0; i < numObject; i++)
	{
		distance = (allObject[i].centroid.x - centroid.x) * (allObject[i].centroid.x - centroid.x) +
					(allObject[i].centroid.y - centroid.y) * (allObject[i].centroid.y - centroid.y);
		if(distance <= RADIUS_MOVING*RADIUS_MOVING) //consider only objects that are inside the safe zone
		{
			int topLeftRoi_x = allObject[i].centroid.x - width/2;
			int topLeftRoi_y = allObject[i].centroid.y - height/2;
			if(topLeftRoi_x < 0)
				topLeftRoi_x = 0;
			if(topLeftRoi_y < 0)
				topLeftRoi_y = 0;

			roiCandidateObject = frame(Rect(topLeftRoi_x, topLeftRoi_y, width, height));

			correValue = correlation(roiObject, roiCandidateObject);

			if(correValue > maxCorre)
			{
				maxCorre = correValue;
				indexMax = i;
			}
		}
	}

	if(indexMax >= 0)
	{
		centroidMeasure.x = allObject[indexMax].centroid.x;
		centroidMeasure.y = allObject[indexMax].centroid.y;
	}else // have no moving object in safe zone
	{
		centroidMeasure.x = centroid.x;
		centroidMeasure.y = centroid.y;
	}
}

float Object::correlation( const Mat &roiObject, const Mat &roiCandidateObject )
{
	Mat grayImage;
	Mat grayTemplate;

	cvtColor(roiCandidateObject, grayImage, CV_RGB2GRAY);
	cvtColor(roiObject, grayTemplate, CV_RGB2GRAY);

	uchar* grayImagePtr;
	uchar* grayTemplatePtr;
	float sum = 0.0;

	for(int i = 0; i < width; i++)
	{
		grayImagePtr = grayImage.ptr<uchar>(i);
		grayTemplatePtr = grayTemplate.ptr<uchar>(i);
		for(int j = 0; j < height; j++)
		{
			sum += (grayImagePtr[j] * grayTemplatePtr[j]) / (width * height);
		}
	}

	return sum;
}

void Object::correct(Mat preFrame, Mat frame, int numObject )
{
	int correctPosition_x = 0;
	int correctPosition_y = 0;

	float distance = (centroidPredict.x - centroidMeasure.x)*(centroidPredict.x - centroidMeasure.x) + 
					(centroidPredict.y - centroidMeasure.y)*(centroidPredict.y - centroidMeasure.y);
	if(distance < 200)
	{
		correctPosition_x = centroidMeasure.x;
		correctPosition_y = centroidMeasure.y;
	}else
	{
		int topLeftRoi_x = centroid.x - 50;
		int topLeftRoi_y = centroid.y - 50;
		if(topLeftRoi_x < 0)
			topLeftRoi_x = 0;
		if(topLeftRoi_y < 0)
			topLeftRoi_y = 0;

// 		int lengthLimit = frame.cols - topLeftRoi_x - 100;
// 		if(lengthLimit < 0 )
// 			lengthLimit = frame.cols - topLeftRoi_x;
// 		else
// 			lengthLimit = 100;

		Mat roiImg = frame(Rect(topLeftRoi_x, topLeftRoi_y, 100, 100));
		//Mat roiImg = frame(Rect(topLeftRoi_x, topLeftRoi_y, lengthLimit, lengthLimit));
		Mat templateImg = preFrame(Rect(topLeft.x, topLeft.y, width, height));
		Point point;
		reMatching(templateImg, roiImg, point); //point is topleft of result rectangle

		//convert to result's centroid 
		correctPosition_x = point.x + centroid.x - 50 + width/2;
		correctPosition_y = point.y + centroid.y - 50 + height/2;
	}


	float distance1 = (centroidPredict.x - correctPosition_x)*(centroidPredict.x - correctPosition_x) + 
					(centroidPredict.y - correctPosition_y)*(centroidPredict.y - correctPosition_y);
	float distance2 = (centroidMeasure.x - correctPosition_x)*(centroidMeasure.x - correctPosition_x) + 
					(centroidMeasure.y - correctPosition_y)*(centroidMeasure.y - correctPosition_y);

	if(distance1 < distance2)
	{
		correctPosition_x = centroidPredict.x ;
		correctPosition_y = centroidPredict.y ;
	}else
	{
		correctPosition_x = centroidMeasure.x;
		correctPosition_y = centroidMeasure.y;
	}


	//update
	vX = correctPosition_x - centroid.x;
	vY = correctPosition_y - centroid.y;

	centroid.x = correctPosition_x;
	centroid.y = correctPosition_y;

	topLeft.x = centroid.x - width/2;
	topLeft.y = centroid.y - height/2;

	downRight.x = centroid.x + width/2;
	downRight.y = centroid.y + height/2;
	
	//update width and height will be done later. Hieu 16_03_2013

 }

void Object::reMatching( Mat templateImg, Mat roiImage, Point &result )
{
	Mat src_base;
	Mat src_test1;
	src_base = templateImg;
	src_test1 = roiImage;

	//imshow("fdf", src_test1);

	int width_base = src_base.cols;
	int height_base =  src_base.rows;

	int width_test = src_test1.cols;
	int height_test = src_test1.rows;

	float value = 0.0;
	float maxValue = 0.0;
	int x = 0;
	int y = 0;
	Mat window;

	for(int i = 0; i < height_test - height_base; i++)
	{
		for(int j = 0; j <  width_test - width_base; j++)
		{
			window = src_test1(Rect(j, i, width_base, height_base));
			value = compareHistogram(src_base, window);
			if(maxValue < value)
			{
				maxValue = value;
				x = j;
				y = i;
			}
		}
	}

	result.x = x;
	result.y = y;



	rectangle(src_test1, Point(x, y), Point(x + width_base,y + height_base), Scalar(255, 0, 0));
	imshow("image", src_test1);
}


float Object::compareHistogram( Mat src_base, Mat src_test1 )
{
	Mat  hsv_base;
	Mat  hsv_test1;

	/// Convert to HSV
	cvtColor( src_base, hsv_base, CV_BGR2HSV );
	cvtColor( src_test1, hsv_test1, CV_BGR2HSV );


	/// Using 30 bins for hue and 32 for saturation
	//int h_bins = 50; int s_bins = 60;
	int h_bins = 10; int s_bins = 10;
	int histSize[] = { h_bins, s_bins };

	// hue varies from 0 to 256, saturation from 0 to 180
	float s_ranges[] = { 0, 256 };
	float h_ranges[] = { 0, 180 };

	const float* ranges[] = { h_ranges, s_ranges };

	// Use the o-th and 1-st channels
	int channels[] = { 0, 1 };


	/// Histograms
	MatND hist_base;
	MatND hist_test1;

	/// Calculate the histograms for the HSV images
	calcHist( &hsv_base, 1, channels, Mat(), hist_base, 2, histSize, ranges, true, false );
	normalize( hist_base, hist_base, 0, 1, NORM_MINMAX, -1, Mat() );

	calcHist( &hsv_test1, 1, channels, Mat(), hist_test1, 2, histSize, ranges, true, false );
	normalize( hist_test1, hist_test1, 0, 1, NORM_MINMAX, -1, Mat() );

	double base_test1 = compareHist( hist_base, hist_test1, 0 );

	//cout <<"base_test1   "<< base_test1 <<endl;

	return base_test1;
}

Point Object::getTopLeft()
{
	Point tmp = topLeft;
	return tmp;
}

Point Object::getDownRight()
{
	Point tmp = downRight;
	return tmp;
}

Point Object::getCentroid()
{
	Point tmp = centroid;
	return tmp;
}

Point Object::getCentroidPredict()
{
	Point tmp = centroidPredict;
	return tmp;
}

Point Object::getCentroidMeasure()
{
	Point tmp = centroidMeasure;
	return tmp;
}
